#include "../include/calibrator.h"
// #define SHOW_CORNERS
bool fytVision::Calibrator::Init(std::vector<cv::Mat>& chessboards_,const cv::Size &img_size_,const cv::Size &board_size_,const cv::Size2f &block_size_)
{
    if(chessboards_.size() <5)
    {
        is_calibrated=false;
        return false;
    }
    
    this->chess_boards.swap(chessboards_);
    this->block_size = block_size_;
    this->img_size = img_size_;
    this->board_size = board_size_;
    return true;
}

bool fytVision::Calibrator::Run()
{
/*获得角点*/
    std::vector<std::vector<cv::Point2f>> img_points;
    for(int i = 0;i<chess_boards.size();++i)
    {
        cv::Mat img_i = chess_boards[i];
        cv::cvtColor(img_i,img_i,cv::COLOR_BGR2GRAY);
        std::vector<cv::Point2f> tmp_pts;
        bool is_corners_found = (
            cv::findChessboardCorners(img_i,board_size,tmp_pts) &&
            cv::find4QuadCornerSubpix(img_i,tmp_pts,cv::Size(5,5))
        );
#ifdef SHOW_CORNERS
        cv::Mat show = img_i.clone();
        cv::drawChessboardCorners(show,board_size,tmp_pts,is_corners_found);
        cv::cvtColor(show,show,cv::COLOR_GRAY2BGR);
        cv::putText(show,std::to_string(i),cv::Point(show.rows/5,show.cols/5),cv::FONT_HERSHEY_COMPLEX,5,cv::Scalar(0,0,255),2);
        cv::imshow("show",show);
        cv::waitKey(0);
#endif
        if(is_corners_found)
        {
            img_points.push_back(tmp_pts);
        }
        else
        {
            std::cout<<std::to_string(i+1)+".jpg"<<"无法使用！请删除"<<std::endl;
        }

    }
#ifdef SHOW_CORNERS
    cv::destroyWindow("show");
#endif
/*生成棋盘格每个内角点的世界坐标*/
    std::vector<std::vector<cv::Point3f>> wld_points;
    for(int i = 0;i<chess_boards.size();++i)
    {
        std::vector<cv::Point3f> tmp_pts;
        for(int j = 0;j<board_size.height;j++)
        {
            for(int k = 0;k<board_size.width;k++)
            {
                cv::Point3f real_pt;
                real_pt.x = j*block_size.width;
                real_pt.y = k*block_size.height;
                real_pt.z = 0;
                tmp_pts.push_back(real_pt);
            }
        }
        wld_points.push_back(tmp_pts);
    }

/*初始化每张图像的角点数量，假定每张图像中都可以看到完整的标定板*/
    std::vector<int> point_number;
    for(int i = 0;i<img_points.size();i++)
    {
        point_number.push_back(board_size.width*board_size.height);
    }

/*开始标定*/
    cv::calibrateCamera(wld_points,img_points,img_size,camera_mat,coeffs,rvecs,tvecs,0);

    is_calibrated = true;
    return true;
}
 
bool fytVision::Calibrator::GetMats(std::vector<cv::Mat>& mats)
{
    if(is_calibrated)
    {
        mats.clear();
        mats.emplace_back(this->camera_mat.clone());
        mats.emplace_back(this->coeffs.clone());
        return true;
    }
    else
        return false;
}